% 5.1 A motivating example in 5 Adaptive Stabilization
% Note:
% (v) Even though μˆ is interpreted as the dynamic estimation of the unknown parameter vector μ, 
% the solvability of the adaptive stabilization problem does not have to imply the convergence of μˆ (t) to μ as t tends to infinity. 
% The quantity μˆ may not even converge at all. (Page. 130)
%
% Z. Chen and J. Huang, Stabilization and regulation of nonlinear systems: a robust and adaptive approach. 
% in Advanced Textbooks in Control and Signal Processing. Cham: Springer International Publishing, 2015. doi: 10.1007/978-3-319-08834-1.


titles=["PE","not PE"];
for ip=1:length(titles)
    params=struct();
    tfinal=10;
    tit=titles(ip);
    switch tit
        case "PE"
            params.f=@(x,t)[sin(t);cos(t)];   % 满足PE条件那么参数收敛
        case "not PE"
            params.f=@(x,t)[1;1];             % 不满足PE条件参数不收敛
    end
    params.mu=[4;6];
    params.rho=2;
    params.Lambda=2*eye(2);
    initial=[2;2;3];
    [d0,s0]=rhs(0,initial,params);
    
    opt=odeset("AbsTol",1e-13,"RelTol",1e-13);
    [t,x]=ode78(@(t,x)rhs(t,x,params),[0 tfinal],initial,opt);
    s=repmat(s0,length(t),1);
    for i=1:length(t)
        [di,si]=rhs(t(i),x(i,:)',params);
        s(i)=si;
    end
    
    tiledlayout(2,1)
    nexttile;hold on;
    plot(t,x(:,1));
    plot(t,abs(horzcat(s.debug1)-horzcat(s.debug2)))
    legend("x","$\|f^T(x,t) \tilde{\mu}\|$","Interpreter","latex")
    title("x")
    grid on
    nexttile;hold on;
    for i=1:length(params.mu)
        p=plot(t,x(:,1+i),"-","DisplayName","$\hat{\mu}_"+i+"$");
        plot([min(t),max(t)],ones(2,1)*params.mu(i),"--","DisplayName","${\mu}_"+i+"$","Color",p.Color);
    end
    legend("Interpreter","latex")
    grid on
    exportgraphics(gcf,tit+"basic.png")
end 
function [dxdt,s]=rhs(t,states,p)
    x=states(1);
    hatmu=states(2:3);
    u=-p.f(x,t)'*hatmu-p.rho*x;
    dhatmu=p.Lambda*x*p.f(x,t);
    dxdt=[p.f(x,t)'*p.mu+u;dhatmu];
    s=struct();
    s.debug1=p.f(x,t)'*p.mu;
    s.debug2=p.f(x,t)'*hatmu;
end